/*
  nav.c
  
  Navigation software for the RCVehicle

  - - - TheStigg - - -
  
 */

// Includes
#include <appled.h>
#include <error.h>
#include <gps.h>
#include <rcmotor.h>
#include <rcsteering.h>
#include <rtos.h>


// Useful Definitions
#define MAX_SPEED   3
#define MAX_LEFT  -25
#define MAX_RIGHT  25

// Local Prototypes
void Nav_ErrState(int led, int flashes);
void Nav_GPSOnly( void *p );

void Nav_ErrState(int led, int flashes)
{
  // If we get in here, we have an error.  Find the corresponding # of flashes.
  int i;
  led %= 4;

  while( true )
    {
      for( i=0; i<flashes; i++ )
	{
	  AppLed_SetState(led,1);
	  Sleep(50);
	  AppLed_SetState(led,0);
	  Sleep(50);
	}
      Sleep( 1000 - 100*flashes );
    }
}

void Nav_GPSOnly( void *p )
{
  // This function reads gps navigation data (if valid), and navigates to a destination.
  (void)p;
  GPS_NAVData NAVData;
  int retVal;
  int speed = 10;   // Set initial speed to 10%

  
  //Init the GPS
  retVal = GPS_SetActive( 1 );
  if( retVal != CONTROLLER_OK )
    Nav_ErrState(0,1);
  /*
  // Init the Motor
  retVal = RCMotor_SetActive( 1 );
  if( retVal != CONTROLLER_OK )
    Nav_ErrState(0,2);

  // Init the Steering
  retVal = RCSteering_SetActive( 1 );
  if( retVal != CONTROLLER_OK )
    Nav_ErrState(0,3);
  */

  // Now with everything started, lets use the system
  do
    {
      // Get GPS Navitaion information
      retVal = GPS_GetNAVData( &NAVData );
      if( retVal != CONTROLLER_OK )
	Nav_ErrState(0,4);

      // Check if the info is Valid
      if( !NAVData.isValid )
	{
	  AppLed_SetState(3,1);
	  Sleep(50);
	  AppLed_SetState(3,0);
	  // then not valid, stop the car
	  RCMotor_SetSpeed( 0 );
	  RCSteering_SetDirection( 0 );
	  continue;
	}
      
      // If we are here, then the data is valid.
      // Adjust the speed
      if( NAVData.velocityTowardsDest < MAX_SPEED )
	speed += 2;
      else
	speed --;

      RCMotor_SetSpeed( speed );

      // Now adjust the steering
      if( NAVData.crossTrackErr == 0.0 )
	RCSteering_SetDirection( 0 );
      else
	{
	  if( NAVData.steeringDir == 'R' )
	    RCSteering_SetDirection( MAX_RIGHT );
	  else
	    RCSteering_SetDirection( MAX_LEFT );
	  
	  Sleep(500);
	  RCSteering_SetDirection( 0 );
	  Sleep(500);

	  if( NAVData.steeringDir == 'R' )
	    RCSteering_SetDirection( MAX_RIGHT * -1 );
	  else
	    RCSteering_SetDirection( MAX_LEFT * -1 );
	  
	  Sleep(500);
	  RCSteering_SetDirection( 0 );
	}
    }
  while( !NAVData.atDest );
  
  RCMotor_Stop();
  RCSteering_SetDirection( 0 );

  AppLed_SetState(0,1);
  for(;;);
}


  
